#include "graph.hpp"
#include "config.hpp"

int BALLID = 1;

bool comp(bbox_t a,bbox_t b){return a.y<b.y;}
bool box_compare(bbox_t one,bbox_t another)
{return(BOXSIZE(one)>BOXSIZE(another));}

void Vision::undistortGenerate(){
	newRaw = 0;
	if(!mapX.empty()){
		remap(raw, undistortASYNC, mapX, mapY, INTER_LINEAR);
		undistortASYNC = undistortASYNC(Range(50, 700), Range(85, 915));
		undistort = undistortASYNC.clone();
	}
	newPic = 1;
}

//get:the first four biggest corner;
//	  the biggest frame
//	  the biggest landmark 
int qualifiedLandmark = 100;
float detectConfidence = 0.6;
void Vision::detectClassifier(bool draw)
{
	if(!undistort.empty()&&newPic)
	{
		this->targetCorners.clear();
		this->targetLandmarks.clear();
		newPic = 0;
		auto boxes = detector->detect(undistort,0.6);
		if(draw){
			drawOn(undistort,boxes);
		}
		vector<bbox_t> tempCorners;
		bbox_t mbox = {0};
		bbox_t mLandMark = {0},mLandMark2 = {0};
		for(auto i:boxes)
		{
			switch(i.obj_id)
			{
				case EXACTBOXID:
					if(BOXSIZE(i)>BOXSIZE(mbox))
						mbox = i;
					targetFrameOrint=0;
					break;
				case LEFTBOXID:
					if(BOXSIZE(i)>BOXSIZE(mbox))
						mbox = i;
					targetFrameOrint=1;
					break;
				case RIGHTBOXID:
					if(BOXSIZE(i)>BOXSIZE(mbox))
						mbox = i;
					targetFrameOrint=-1;
					break;
				case BLUE_BALLID:
					if(BALLID==1)
						targetBall = i;
					break;
				case RED_BALLID:
					if(BALLID==0)
						targetBall = i;
					break;
				case L_LANDMARKID:
					if(BOXSIZE(i)>BOXSIZE(mLandMark))
						mLandMark = i;
					if(BOXSIZE(i)>qualifiedLandmark)
						this->targetLandmarks.push_back(i);
					break;
				case R_LANDMARKID:
					if(BOXSIZE(i)>BOXSIZE(mLandMark))
						mLandMark = i;
					if(BOXSIZE(i)>qualifiedLandmark)
						this->targetLandmarks.push_back(i);
					break;
				case E_LANDMARKID:
					if(BOXSIZE(i)>BOXSIZE(mLandMark))
						mLandMark = i;
					if(BOXSIZE(i)>qualifiedLandmark)
						this->targetLandmarks.push_back(i);
					break;
				case CORNERID:
					targetCorners.push_back(i);
					break;
				default:
					break;
			}
		}
		if(targetCorners.size()>1)
			std::sort(targetCorners.begin(),targetCorners.end(),box_compare);
		if(targetLandmarks.size()>1)
			std::sort(targetLandmarks.begin(),targetLandmarks.end(),box_compare);
		this->targetFrame = mbox;
		this->targetLandmark = mLandMark;
		
	}
}

Vision::Vision(VideoCapture * cap,string xmlfilename,Detector* detr)
{
	this->cam = cap;
	FileStorage fs(xmlfilename, FileStorage::READ);
	Mat intrinsics, distortion;
	fs["camera_matrix"] >> intrinsics;
	fs["distortion_coefficients"] >> distortion;
	fs.release();
	initUndistortRectifyMap(intrinsics, distortion, Mat(),
		getOptimalNewCameraMatrix(intrinsics, distortion, Size(640, 480), 1, Size(1000, 750), 0),
		Size(1000, 750),
		CV_16SC2, mapX, mapY);
	this->detector = detr;
}

inline Vision::Vision(VideoCapture * cap,Detector* detr){
	this->cam = cap;
	Mat temp;
	if(cap->isOpened()){
		cap->read(temp);
		this->HEIGHT = temp.size().height;
		this->WIDTH = temp.size().width;
		this->detector = detr;
	}
}

void Vision::feedInPic(){
	Mat temp;
	if(cam->isOpened()){
		this->newRaw = 1;
		cam->read(raw);
		if(WIDTH+HEIGHT==0){
			WIDTH = raw.size().width;
			HEIGHT = raw.size().height;
		}
	}
}

void Vision::feedInPic(Mat& captured){
	Mat temp;
	if(!captured.empty()){
		this->newRaw = 1;
		raw = captured.clone();
		WIDTH = captured.size().width;
		HEIGHT = captured.size().height;
	}
}
